#include "_includes.h"
bool need_relaxing = false;

// 后台测试自动，可以左键停止
void run_auto_background() {
  autonomous_running = true;
  ActionUnit::set_all_units_motor_stop_status_on_working();
  ////////////////////////
  vex::thread th_auto_test(_autonomous);
  wait(10);  // wait for start
  // 等待自动程序完毕
  while (th_auto_test.joinable()) {
    if (special_key.pressing()) {  // 强制停止自动程序
      thread_autonomous.interrupt();  // 停止场控启动的自动程序后台线程
      th_auto_test.interrupt();      // 停止自动程序后台线程
      ActionUnit::all_units_stop();  // 所有运动单元停转
      chassis.stop();
      ctrl.Screen.setCursor(3, 1);
      ctrl.Screen.print("auto terminated");
      while (special_key.pressing()) wait(5);
      ActionUnit::set_all_units_motor_stop_status_on_relaxing();
    }  // 强制停止自动程序
    wait(100);
  }  // 自动程序测试完毕

  // 正常运行完毕
  autonomous_running = false;
  ActionUnit::set_all_units_motor_stop_status_on_relaxing();
  ActionUnit::all_units_stop();
}

// 底盘马达测试,按下转，松开停，再按反转
#if USE_ControllerEnhanced
void chassis_motor_test(ControllerEnhanced::button_enhanced &b, motor &m,
                        int &dir) {
#else
void chassis_motor_test(const controller::button &b, motor &m, int &dir) {
#endif
  if (b.pressing()) {
    need_relaxing = false;
    m.spin(directionType::fwd, 12 * dir, voltageUnits::volt);
    dir = -dir;
    while_wait(b.pressing());
    m.stop();
  }
}

extern bool screen_ready;
extern bool AUTO_TEST;
extern void route_number_roll_over();
void special_key_handle() {
  // 左键按下
  if (special_key.pressing()) {
    autonomous_running = false;
    need_relaxing = true;
    wait(10);
    ActionUnit::all_units_stop();  // 所有运动单元停转

    int dir_lf = 1, dir_lb = 1, dir_rf = 1, dir_rb = 1;  // 底盘马达测试旋转方向
    int dir_l3 = 1, dir_r3 = 1;  // 6底盘马达测试旋转方向
    while (special_key.pressing()) {
      wait(10);  // cpu schedule
      /*
      1:自动程序测试
      */
      if (ctrl.ButtonRight.pressing()) {  // key right
        need_relaxing = false;
        while_wait(ctrl.ButtonRight.pressing() ||
                   special_key.pressing());  // 等待松开
        run_auto_background();
      }  // 1:自动程序测试

      /*
      2:底盘马达测试
      */
      if (1) {
        chassis_motor_test(ctrl.ButtonUp, mtr_LF, dir_lf);
        chassis_motor_test(ctrl.ButtonDown, mtr_LB, dir_lb);
        chassis_motor_test(ctrl.ButtonX, mtr_RF, dir_rf);
        chassis_motor_test(ctrl.ButtonB, mtr_RB, dir_rb);
        if (chassis_l.get_motor_count() == 3)
          chassis_motor_test(ctrl.ButtonL1, *chassis_l.vec_ptr_motors[2],
                             dir_l3);
        if (chassis_r.get_motor_count() == 3)
          chassis_motor_test(ctrl.ButtonR1, *chassis_r.vec_ptr_motors[2],
                             dir_r3);
      }  // 2:底盘马达测试

      /*
      3:显示区页面切换
      */
      if (screen_ready) {
        int switch_value = 40;
        if (ctrl.Axis2.position() >= switch_value) {
          need_relaxing = false;
          page_up();
          while_wait(ctrl.Axis2.position() >= switch_value);
        }
        if (ctrl.Axis2.position() <= -switch_value) {
          need_relaxing = false;
          page_down();
          while_wait(ctrl.Axis2.position() <= -switch_value);
        }
      }  // 3:显示区页面切换

      /*
      4:自动路线选择
      */
      if (screen_ready) {
        int switch_value = 40;
        // 红蓝场
        if (ctrl.Axis3.position() >= switch_value) {
          need_relaxing = false;
          AUTO_TEST = false;
          auto_info.alliance = Alliance::red;
          update_side_role_button_according_to_auto_info();
        }
        if (ctrl.Axis3.position() <= -switch_value) {
          need_relaxing = false;
          AUTO_TEST = false;
          auto_info.alliance = Alliance::blue;
          update_side_role_button_according_to_auto_info();
        }
        // 角色场
        if (ctrl.Axis4.position() <= -switch_value) {
          need_relaxing = false;
          AUTO_TEST = false;
          auto_info.side = Side::left;
          update_side_role_button_according_to_auto_info();
        }
        if (ctrl.Axis4.position() >= switch_value) {
          need_relaxing = false;
          AUTO_TEST = false;
          auto_info.side = Side::right;
          update_side_role_button_according_to_auto_info();
        }
        // 路线号
        if (ctrl.Axis1.position() <= -switch_value) {
          need_relaxing = false;
          AUTO_TEST = false;
          auto_info.route_number--;
          route_number_roll_over();
          update_side_role_button_according_to_auto_info();
          while_wait(ctrl.Axis1.position() <= -switch_value);
        }
        if (ctrl.Axis1.position() >= switch_value) {
          need_relaxing = false;
          AUTO_TEST = false;
          auto_info.route_number++;
          route_number_roll_over();
          update_side_role_button_according_to_auto_info();
          while_wait(ctrl.Axis1.position() >= switch_value);
        }
      }  // 4:自动路线选择

      /*
      5: 所有马达编码器和陀螺仪归零
      */
      if (ctrl.ButtonA.pressing()) {
        need_relaxing = false;
        /////////////////
        screen_clear();
        buttons_show_up();
        ActionUnit::all_units_reset_enc();
        imu.resetRotation();
        while (ctrl.ButtonA.pressing()) {
          ctrl_rumble_remind_reset();  // 振动提示重新计时
          wait(5);
        }
        competition_on = true;
        ctrl_clock_disp_reset();
        ctrl.rumble(".");
      }
      /*
      6: 特殊操作
      */
    }  // while(key left)

    // 松开左键后
    if (need_relaxing) {
      // extern bool pitch_anti_slip;
      // pitch_anti_slip=false;
      ////////////////////////////////
      timer t;
      controller_print_team_number_and_autoinfo();
      // 300ms之内检测再次按下左键,则取消自动休眠
      while (t.time() < 300) {
        if (special_key.pressing()) {
          need_relaxing = false;
          while_wait(special_key.pressing());
          break;
        }
        wait(5);
      }

      if (need_relaxing == false) {  // 300ms快速双击取消自动休眠
        ctrl.rumble("--");
        ActionUnit::b_auto_relaxing_on = false;
        ActionUnit::set_all_units_motor_stop_status_on_working();
      } else {  // 单击则立即休眠
        ctrl.rumble(".");
        ActionUnit::b_auto_relaxing_on = true;
        ActionUnit::set_all_units_motor_stop_status_on_relaxing();
      }
    }
    /////////////////////////////
  }  // if key left
}
